
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gcs_mission.c
  * @author     baiyang
  * @date       2022-10-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <gcs_mavlink/gcs.h>

#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void send_mission_ack(mavlink_message_t *msg, uint8_t chan, MAV_MISSION_RESULT result);
static bool handle_mission_item(mavlink_message_t *msg, uint8_t chan);
static bool handle_guided_request(mission_command_t cmd);
static void do_yaw(const mission_command_t cmd);
static void queued_waypoint_send(mavlink_message_t *msg, uint8_t chan);
static void send_mission_count(mavlink_message_t *msg, uint16_t count, uint8_t chan);
static void handle_mission_count(mavlink_message_t *msg, uint8_t chan);
static void handle_mission_clear_all(mavlink_message_t *msg, uint8_t chan);
static void handle_mission_request_list(mavlink_message_t *msg, uint8_t chan);
static void send_mission_item_int(mavlink_message_t *msg, mavlink_mission_item_int_t *mission_item_int, uint8_t chan);
static void send_mission_item(mavlink_message_t *msg, mavlink_mission_item_t *mission_item, uint8_t chan);
static void handle_mission_request(mavlink_message_t *msg, uint8_t chan);
static void handle_mission_set_current(mavlink_message_t *msg, uint8_t chan);
static void handle_mission_write_partial_list(mavlink_message_t *msg, uint8_t chan);
/*----------------------------------variable----------------------------------*/
bool            waypoint_receiving = 0; // currently receiving

// the following two variables are only here because of Tracker
uint16_t        waypoint_request_i = 0; // request index
uint16_t        waypoint_request_last = 0; // last request index

// waypoints
uint16_t        waypoint_dest_sysid = 0; // where to send requests
uint16_t        waypoint_dest_compid = 0; // "
uint16_t        waypoint_count = 0;
uint32_t        waypoint_timelast_receive = 0; // milliseconds
uint32_t        waypoint_timelast_request = 0; // milliseconds
const uint16_t  waypoint_receive_timeout = 8000; // milliseconds

static mavlink_message_t msg_push;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       任务回复
  * @param[in]   msg  
  * @param[out]  
  * @retval      
  * @note        mavlink发送或打包函数中的sysid和compid是飞控的，
  * 结构体中的，是地面站的
  */
static void send_mission_ack(mavlink_message_t *msg, uint8_t chan,
                      MAV_MISSION_RESULT result)
{
    MAV_CHECK_PAYLOAD_SIZE2_VOID(chan, MISSION_ACK);
    mavlink_msg_mission_ack_send(chan,
                                 msg->sysid,
                                 msg->compid,
                                 result,
                                 MAV_MISSION_TYPE_MISSION);
}

/// mission
/*
  handle an incoming mission item
  return true if this is the last mission item, otherwise false
 */
static bool handle_mission_item(mavlink_message_t *msg, uint8_t chan)
{
    MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED;
    bool mission_is_complete = false;
    // TODO: rename packet to mission_item_int
    mavlink_mission_item_int_t packet = {0};
    struct Mission_Command cmd = {0};
    uint16_t seq = 0;
    uint8_t current = 0;

    if (msg->msgid == MAVLINK_MSG_ID_MISSION_ITEM) {
        mavlink_mission_item_t mission_item;
        mavlink_msg_mission_item_decode(msg, &mission_item);
        MAV_MISSION_RESULT ret = mission_convert_MISSION_ITEM_to_MISSION_ITEM_INT(&mission_item, &packet);
        ret = mission_mavlink_int_to_mission_cmd(&packet, &cmd);
        if (ret != MAV_MISSION_ACCEPTED) {
            goto mission_ack;
        }
    } else {
        mavlink_msg_mission_item_int_decode(msg, &packet);
        MAV_MISSION_RESULT ret = mission_mavlink_int_to_mission_cmd(&packet, &cmd);
        if (ret != MAV_MISSION_ACCEPTED) {
            goto mission_ack;
        }
    }
    current = packet.current;
    seq = packet.seq;

    if ((current == 2 || current == 3)) {
        MAV_MISSION_RESULT result = mission_mavlink_int_to_mission_cmd(&packet, &cmd);
        if (result != MAV_MISSION_ACCEPTED) {
            //decode failed
            goto mission_ack;
        }
        // guided or change-alt
        if (current == 2) {
            // current = 2 is a flag to tell us this is a "guided mode"
            // waypoint and not for the mission
            result = (handle_guided_request(&cmd) ? MAV_MISSION_ACCEPTED
                      : MAV_MISSION_ERROR) ;
        } else if (current == 3) {
            //current = 3 is a flag to tell us this is a alt change only
            // add home alt if needed

            // verify we recevied the command
            result = MAV_MISSION_ACCEPTED;
        }
        goto mission_ack;
    }

    // Check if receiving waypoints (mission upload expected)
    if (!waypoint_receiving) {
        result = MAV_MISSION_ERROR;
        goto mission_ack;
    }

    // check if this is the requested waypoint
    if (seq != waypoint_request_i) {
        result = MAV_MISSION_INVALID_SEQUENCE;
        goto mission_ack;
    }

    // sanity check for DO_JUMP command
    if (cmd.id == MAV_CMD_DO_JUMP) {
        if ((cmd.content.jump.target >= mission_num_commands() && cmd.content.jump.target >= waypoint_request_last) || cmd.content.jump.target == 0) {
            result = MAV_MISSION_ERROR;
            goto mission_ack;
        }
    }
    
    
    // if command index is within the existing list, replace the command
    if (seq < mission_num_commands()) {
        if (mission_replace_cmd(seq, &cmd)) {
            result = MAV_MISSION_ACCEPTED;
        }else{
            result = MAV_MISSION_ERROR;
            goto mission_ack;
        }
        // if command is at the end of command list, add the command
    } else if (seq == mission_num_commands()) {
        if (mission_add_cmd(&cmd)) {
            result = MAV_MISSION_ACCEPTED;
        }else{
            result = MAV_MISSION_ERROR;
            goto mission_ack;
        }
        // if beyond the end of the command list, return an error
    } else {
        result = MAV_MISSION_ERROR;
        goto mission_ack;
    }
    
    // update waypoint receiving state machine
    waypoint_timelast_receive = time_millis();
    waypoint_request_i++;

    if (waypoint_request_i >= waypoint_request_last) {
        send_mission_ack(msg, chan, MAV_MISSION_ACCEPTED);
        //send_text(MAV_SEVERITY_INFO,"Flight plan received");
        //mavlink_send_status(MISSION_RECEIVE_OK);
        waypoint_receiving = false;
        mission_is_complete = true;
        // XXX ignores waypoint radius for individual waypoints, can
        // only set WP_RADIUS parameter
    } else {
        waypoint_timelast_request = time_millis();
        queued_waypoint_send(msg, chan);
    }

    return mission_is_complete;
mission_ack:
    // we are rejecting the mission/waypoint
    send_mission_ack(msg, chan, result);

    return mission_is_complete;
}

static bool handle_guided_request(mission_command_t cmd)
{
    // only process guided waypoint if we are in guided mode
    if (mode_number(fms.flightmode) != GUIDED) {
       return false;
    }

    // switch to handle different commands
    switch (cmd->id) {

        case MAV_CMD_NAV_WAYPOINT:
        {
          // set wp_nav's destination
          Location dest = cmd->content.location;
          return mguided_set_destination2((mode_base_t)&fms.mode_guided, &dest, false, 0.0f, false, 0.0f, false);
        }

        case MAV_CMD_CONDITION_YAW:
          do_yaw(cmd);
          return true;

        default:
          // reject unrecognised command
          return false;
    }

    return true;
}

static void do_yaw(const mission_command_t cmd)
{
    autoyaw_set_fixed_yaw(fms.flightmode->auto_yaw,
        cmd->content.yaw.angle_deg,
        cmd->content.yaw.turn_rate_dps,
        cmd->content.yaw.direction,
        cmd->content.yaw.relative_angle > 0);
}

/**
* @brief Send the next pending waypoint, called from deferred message
* handling code
*/
static void queued_waypoint_send(mavlink_message_t *msg, uint8_t chan)
{
    if (waypoint_receiving &&
        waypoint_request_i <= waypoint_request_last) {
        mavlink_msg_mission_request_int_send(chan, msg->sysid, msg->compid, waypoint_request_i, MAV_MISSION_TYPE_MISSION);
    }
}

static void send_mission_count(mavlink_message_t *msg, uint16_t count, uint8_t chan)
{
    mavlink_msg_mission_count_send(chan, msg->sysid, msg->compid, count, MAV_MISSION_TYPE_MISSION);
}

/*
  handle a MISSION_COUNT mavlink packet
 */
static void handle_mission_count(mavlink_message_t *msg, uint8_t chan)
{
    // decode
    mavlink_mission_count_t packet = {0};
    mavlink_msg_mission_count_decode(msg, &packet);

    // start waypoint receiving
    if (packet.count > mission_num_commands_max()) {
        // send NAK
        send_mission_ack(msg, chan, MAV_MISSION_NO_SPACE);
        return;
    }

    // new mission arriving, truncate mission to be the same length
    mission_truncate(packet.count);

    // set variables to help handle the expected receiving of commands from the GCS
    waypoint_timelast_receive = time_millis();    // set time we last received commands to now
    waypoint_receiving = true;              // record that we expect to receive commands
    waypoint_request_i = 0;                 // reset the next expected command number to zero
    waypoint_request_last = packet.count;   // record how many commands we expect to receive
    waypoint_timelast_request = 0;          // set time we last requested commands to zero

    waypoint_dest_sysid = msg->sysid; // where to send requests
    waypoint_dest_compid = msg->compid; // "
    queued_waypoint_send(msg, chan);
}

/*
  handle a MISSION_CLEAR_ALL mavlink packet
 */
static void handle_mission_clear_all(mavlink_message_t *msg, uint8_t chan)
{
    // decode
    mavlink_mission_clear_all_t packet = {0};
    mavlink_msg_mission_clear_all_decode(msg, &packet);

    // clear all waypoints
    if (mission_clear()) {
        // send ack
        send_mission_ack(msg, chan, MAV_MISSION_ACCEPTED);
    }else{
        // send nack
        send_mission_ack(msg, chan, MAV_MISSION_ERROR);
    }
}

/*
  handle a MISSION_REQUEST_LIST mavlink packet
 */
static void handle_mission_request_list(mavlink_message_t *msg, uint8_t chan)
{
    // decode
    mavlink_mission_request_list_t packet = {0};
    mavlink_msg_mission_request_list_decode(msg, &packet);

    // reply with number of commands in the mission.  The GCS will then request each command separately
    send_mission_count(msg, mission_num_commands(), chan);

    // set variables to help handle the expected sending of commands to the GCS
    waypoint_receiving = false;             // record that we are sending commands (i.e. not receiving)
    waypoint_dest_sysid = msg->sysid;       // record system id of GCS who has requested the commands
    waypoint_dest_compid = msg->compid;     // record component id of GCS who has requested the commands
}

static void send_mission_item_int(mavlink_message_t *msg,
                      mavlink_mission_item_int_t *mission_item_int, uint8_t chan)
{
    mavlink_msg_mission_item_int_send_struct(chan, mission_item_int);
}

static void send_mission_item(mavlink_message_t *msg,
                    mavlink_mission_item_t *mission_item, uint8_t chan)
{
    mavlink_msg_mission_item_send_struct(chan, mission_item);
}


/*
  handle a MISSION_REQUEST mavlink packet
 */
static void handle_mission_request(mavlink_message_t *msg, uint8_t chan)
{
    struct Mission_Command cmd = {0};

    if (msg->msgid == MAVLINK_MSG_ID_MISSION_REQUEST_INT) {  
        // decode
        mavlink_mission_request_int_t packet = {0};
        mavlink_msg_mission_request_int_decode(msg, &packet);

        // retrieve mission from eeprom
        if (!mission_read_cmd_from_storage(packet.seq, &cmd)) {
            goto mission_item_send_failed;
        }

        mavlink_mission_item_int_t ret_packet = {0};
        if (!mission_mission_cmd_to_mavlink_int(&cmd, &ret_packet)) {
            goto mission_item_send_failed;
        }

        // set packet's current field to 1 if this is the command being executed
        if (cmd.id == (uint16_t)mission_get_current_nav_cmd()->index) {
            ret_packet.current = 1;
        } else {
            ret_packet.current = 0;
        }

        // set auto continue to 1
        ret_packet.autocontinue = 1;     // 1 (true), 0 (false)

        /*
          avoid the _send() function to save memory, as it avoids
          the stack usage of the _send() function by using the already
          declared ret_packet above
         */
        ret_packet.target_system = msg->sysid;
        ret_packet.target_component = msg->compid;
        ret_packet.seq = packet.seq;
        ret_packet.mission_type = packet.mission_type;

        send_mission_item_int(msg, &ret_packet, chan);
    } else {
        // decode
        mavlink_mission_request_t packet = {0};
        mavlink_msg_mission_request_decode(msg, &packet);

        if (packet.seq != 0 && // always allow HOME to be read
            packet.seq >= mission_num_commands()) {
            // try to educate the GCS on the actual size of the mission:
            send_mission_count(msg, mission_num_commands(), chan);
            goto mission_item_send_failed;
        }

        // retrieve mission from eeprom
        if (!mission_read_cmd_from_storage(packet.seq, &cmd)) {
            goto mission_item_send_failed;
        }

        mavlink_mission_item_t ret_packet = {0};
        mavlink_mission_item_int_t ret_packet_int = {0};

        bool ret = mission_mission_cmd_to_mavlink_int(&cmd, &ret_packet_int);
        if (ret != true) {
            goto mission_item_send_failed;
        }
        
        MAV_MISSION_RESULT ret2 = mission_convert_MISSION_ITEM_INT_to_MISSION_ITEM(&ret_packet_int, &ret_packet);
        if (ret2 != MAV_MISSION_ACCEPTED) {
            goto mission_item_send_failed;
        }

        // set packet's current field to 1 if this is the command being executed
        if (cmd.id == (uint16_t)mission_get_current_nav_cmd()->index) {
            ret_packet.current = 1;
        } else {
            ret_packet.current = 0;
        }

        // set auto continue to 1
        ret_packet.autocontinue = 1;     // 1 (true), 0 (false)

        /*
          avoid the _send() function to save memory, as it avoids
          the stack usage of the _send() function by using the already
          declared ret_packet above
         */
        ret_packet.target_system = msg->sysid;
        ret_packet.target_component = msg->compid;
        ret_packet.seq = packet.seq;
        ret_packet.mission_type = packet.mission_type;

        send_mission_item(msg, &ret_packet, chan);
    }

    return;

mission_item_send_failed:
    // send failure message
    send_mission_ack(msg, MAV_MISSION_ERROR, chan);
}

/*
  handle a MISSION_SET_CURRENT mavlink packet
 */
static void handle_mission_set_current(mavlink_message_t *msg, uint8_t chan)
{
    // decode
    mavlink_mission_set_current_t packet = {0};
    mavlink_msg_mission_set_current_decode(msg, &packet);

    // set current command
    if (mission_set_current_cmd(packet.seq, false)) {
        
        // because MISSION_SET_CURRENT is a message not a command,
        // there is not ACK associated with us successfully changing
        // our waypoint.  Some GCSs use the fact we return exactly the
        // same mission sequence number in this packet as an ACK - so
        // if they send a MISSION_SET_CURRENT with seq number of 4
        // then they expect to receive a MISSION_CURRENT message with
        // exactly that sequence number in it, even if ArduPilot never
        // actually holds that as a sequence number (e.g. packet.seq==0).
        if (MAV_HAVE_PAYLOAD_SPACE(chan, MISSION_CURRENT)) {
            mavlink_msg_mission_current_send(chan, packet.seq, mission_num_commands(), 0, 0);
        } else {
            // schedule it for later:
            gcs_send_message2(chan, MAV_MSG_CURRENT_WAYPOINT);
        }
    }
}

/*
  handle a MISSION_WRITE_PARTIAL_LIST mavlink packet
 */
static void handle_mission_write_partial_list(mavlink_message_t *msg, uint8_t chan)
{
    // decode
    mavlink_mission_write_partial_list_t packet = {0};
    mavlink_msg_mission_write_partial_list_decode(msg, &packet);

    // start waypoint receiving
    if ((unsigned)packet.start_index > mission_num_commands() ||
        (unsigned)packet.end_index > mission_num_commands() ||
        packet.end_index < packet.start_index) {
        gcs_send_text2(chan, MAV_SEVERITY_WARNING,"Flight plan update rejected");
        return;
    }

    waypoint_timelast_receive = time_millis();
    waypoint_timelast_request = 0;
    waypoint_receiving   = true;
    waypoint_request_i   = packet.start_index;
    waypoint_request_last= packet.end_index;

    waypoint_dest_sysid = msg->sysid; // where to send requests
    waypoint_dest_compid = msg->compid; // "
}

void fms_mission_item_reached_send(uint16_t index)
{
    gcs_send_mission_item_reached_message(index);
}

/**
* @brief       和地面战交互上传下载航线入口函数
* @param[in]   msg  
* @param[out]  
* @retval      
* @note        
*/
void fms_handle_common_mission_message(mavlink_message_t *msg, uint8_t chan)
{

    switch (msg->msgid) {
        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
        {
            handle_mission_write_partial_list(msg, chan);
            break;
        }

        // GCS has sent us a mission item, store to EEPROM
        case MAVLINK_MSG_ID_MISSION_ITEM:           // MAV ID: 39
        case MAVLINK_MSG_ID_MISSION_ITEM_INT:
            handle_mission_item(msg, chan);
            break;

        // read an individual command from EEPROM and send it to the GCS
        case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST:
            handle_mission_request(msg, chan);
            break;

        case MAVLINK_MSG_ID_MISSION_SET_CURRENT:    // MAV ID: 41
        {
            handle_mission_set_current(msg, chan);
            break;
        }

        // GCS request the full list of commands, we return just the number and leave the GCS to then request each command individually
        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:       // MAV ID: 43
        {
            handle_mission_request_list(msg, chan);
            break;
        }

        // GCS provides the full number of commands it wishes to upload
        //  individual commands will then be sent from the GCS using the MAVLINK_MSG_ID_MISSION_ITEM message
        case MAVLINK_MSG_ID_MISSION_COUNT:          // MAV ID: 44
        {
            handle_mission_count(msg, chan);
            break;
        }

        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:      // MAV ID: 45
        {
            handle_mission_clear_all(msg, chan);
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ACK:
            /* not used */
            break;
    }
}

/*------------------------------------test------------------------------------*/


